/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/


#ifndef TRACKING_H
#define TRACKING_H

#include<opencv2/core/core.hpp>
#include<opencv2/features2d/features2d.hpp>

#include"Viewer.h"
#include"FrameDrawer.h"
#include"Map.h"
#include"LocalMapping.h"
#include"LoopClosing.h"
#include"Frame.h"
#include "ORBVocabulary.h"
#include"KeyFrameDatabase.h"
#include"ORBextractor.h"
#include "Initializer.h"
#include "MapDrawer.h"
#include "System.h"

#include <mutex>

namespace ORB_SLAM2
{

class Viewer;

class FrameDrawer;

class Map;

class LocalMapping;

class LoopClosing;

class System;

class Tracking
{

public:

  Tracking(System *pSys, ORBVocabulary *pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap,
           KeyFrameDatabase *pKFDB, const string &strSettingPath, const int sensor, const bool bUseGT);

  // Preprocess the input and call Track(). Extract features and performs stereo matching.

  cv::Mat GrabImageStereoSemantic(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const cv::Mat &imSeg,const double &timestamp);

  void CalDynamicProposal(const cv::Mat &mImCurrentGray, const cv::Mat &mImLastGray,
                          std::vector<cv::Point2f> &vDynamicProposal);

  void CheckProposal(std::vector<cv::Point2f> &vDynamicProposal);

  void SetLocalMapper(LocalMapping *pLocalMapper);

  void SetLoopClosing(LoopClosing *pLoopClosing);

  void SetViewer(Viewer *pViewer);

  // Load new settings
  // The focal lenght should be similar or scale prediction will fail when projecting points
  // TODO: Modify MapPoint::PredictScale to take into account focal lenght
  void ChangeCalibration(const string &strSettingPath);

  // Use this function if you have deactivated local mapping and you only want to localize the camera.
  void InformOnlyTracking(const bool &flag);

  cv::Mat DbugDrawDynamicProposal(const vector<cv::Point2f> &vDynamicProposal);

  inline float GetDepthThreshold()
  { return mThDepth; }

public:

  // Tracking states
  enum eTrackingState
  {
  SYSTEM_NOT_READY = -1,
  NO_IMAGES_YET = 0,
  NOT_INITIALIZED = 1,
  OK = 2,
  LOST = 3
  };

  eTrackingState mState;
  eTrackingState mLastProcessedState;

  // Input sensor
  int mSensor;

  // Current Frame
  Frame mCurrentFrame;
  cv::Mat mImGray;
  cv::Mat mImCurrentSegment;
  cv::Mat mImLastGray;
  cv::Mat mImDebugDynamicProposal;
  cv::Mat mImDebugOptiFlow;
  // Initialization Variables (Monocular)
  std::vector<int> mvIniLastMatches;
  std::vector<int> mvIniMatches;
  std::vector<cv::Point2f> mvbPrevMatched;
  std::vector<cv::Point3f> mvIniP3D;
  Frame mInitialFrame;

  // Lists used to recover the full camera trajectory at the end of the execution.
  // Basically we store the reference keyframe for each frame and its relative transformation
  list <cv::Mat> mlRelativeFramePoses;
  list<KeyFrame *> mlpReferences;
  list<double> mlFrameTimes;
  list<bool> mlbLost;

  // True if local mapping is deactivated and we are performing only localization
  bool mbOnlyTracking;
  cv::Mat mK;
  float mbf;

  void Reset();

protected:

  // Main tracking function. It is independent of the input sensor.
  void Track();

  // Map initialization for stereo and RGB-D
  void StereoInitialization();

  // Map initialization for monocular
  void MonocularInitialization();

  void CreateInitialMapMonocular();

  void CheckReplacedInLastFrame();

  bool TrackReferenceKeyFrame();

  void UpdateLastFrame();

  bool TrackWithMotionModel();

  bool Relocalization();

  void UpdateLocalMap();

  void UpdateLocalPoints();

  void UpdateLocalKeyFrames();

  bool TrackLocalMap();

  void SearchLocalPoints();

  bool NeedNewKeyFrame();

  void CreateNewKeyFrame();

  // In case of performing only localization, this flag is true when there are no matches to
  // points in the map. Still tracking will continue if there are enough matches with temporal points.
  // In that case we are doing visual odometry. The system will try to do relocalization to recover
  // "zero-drift" localization to the map.
  bool mbVO;

  //Other Thread Pointers
  LocalMapping *mpLocalMapper;
  LoopClosing *mpLoopClosing;

  //ORB
  ORBextractor *mpORBextractorLeft, *mpORBextractorRight;
  ORBextractor *mpIniORBextractor;

  //BoW
  ORBVocabulary *mpORBVocabulary;
  KeyFrameDatabase *mpKeyFrameDB;

  // Initalization (only for monocular)
  Initializer *mpInitializer;

  //Local Map
  KeyFrame *mpReferenceKF;
  std::vector<KeyFrame *> mvpLocalKeyFrames;
  std::vector<MapPoint *> mvpLocalMapPoints;

  // System
  System *mpSystem;

  //Drawers
  Viewer *mpViewer;
  FrameDrawer *mpFrameDrawer;
  MapDrawer *mpMapDrawer;

  bool mbPause;
  //Map
  Map *mpMap;

  //Calibration matrix
  cv::Mat mDistCoef;

  //New KeyFrame rules (according to fps)
  int mMinFrames;
  int mMaxFrames;

  // Threshold close/far points
  // Points seen as close by the stereo/RGBD sensor are considered reliable
  // and inserted from just one frame. Far points requiere a match in two keyframes.
  float mThDepth;

  // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
  float mDepthMapFactor;

  //Current matches in frame
  int mnMatchesInliers;

  //Last Frame, KeyFrame and Relocalisation Info
  KeyFrame *mpLastKeyFrame;
  Frame mLastFrame;
  unsigned int mnLastKeyFrameId;
  unsigned int mnLastRelocFrameId;

  //Motion Model
  cv::Mat mVelocity;

  //Color order (true RGB, false BGR, ignored if grayscale)
  bool mbRGB;

  bool mbUseGT;

  list<MapPoint *> mlpTemporalPoints;
};

} //namespace ORB_SLAM

#endif // TRACKING_H
